//
// Created by xuwj on 2021/11/10.
//

#ifndef CV_ASSISTANT_FILTERUTILITIES_H
#define CV_ASSISTANT_FILTERUTILITIES_H

#include "chrono"
#include "thread"
#include "mutex"
#include "cmath"
#include "PointcloudProcessing/common_xu.h"

#include "opencv2/opencv.hpp"

//filters that remove outliers
#include "pcl/filters/radius_outlier_removal.h"
#include "pcl/filters/conditional_removal.h"
#include "pcl/filters/statistical_outlier_removal.h"
#include "pcl/filters/passthrough.h"

//Model-based Operations
#include "pcl/point_types_conversion.h"
#include "pcl/ModelCoefficients.h"
#include "pcl/sample_consensus/method_types.h"
#include "pcl/sample_consensus/model_types.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/filters/extract_indices.h"
#include "pcl/filters/project_inliers.h"
#include "pcl/segmentation//sac_segmentation.h"

//Visualization
#include "pcl/visualization/cloud_viewer.h"
#include "pcl/visualization/pcl_visualizer.h"

namespace CV_Assistant {
namespace PointcloudProcessing {
    using namespace std;

    const std::string default_viewer_name = "PointCloud_Viewer";
    const double free_value = -77777;

    void viewerOneOffA(pcl::visualization::PCLVisualizer& viewer)
    {
        viewer.setBackgroundColor(1.0, 1.0, 1.0);
        viewer.setCameraPosition(-30.0,30.0,0.0,-1.0,0.0,0.0);
    }

    void viewerOneOffB(pcl::visualization::PCLVisualizer& viewer)
    {
        viewer.setBackgroundColor(1.0, 1.0, 1.0);
    }

    template<typename pointT>
    class FiltersHelper{
    public:
        typedef pcl::PointCloud<pointT> CloudT;
        typedef boost::shared_ptr< pcl::PointCloud<pointT>> CloudT_Ptr;

        /**
         * you need a pcd file or PointCloud to initialize the helper
         * @param pcd_file
         */
        FiltersHelper(const std::string &pcd_file):
        keyboard_flag_(-1),callKeyboardStop_(false){
            for (int i = 0; i < 6; ++i) {
                keyboard_value_[i] = free_value;
            }
            pcloud_origin_ = CloudT_Ptr (new CloudT);
            pcloud_ = CloudT_Ptr (new CloudT);

            pcl::PCDReader reader;
            reader.read(pcd_file,*pcloud_origin_);
            if(pcloud_origin_->empty()){
                cout<<"Error: the input pcd file is empty!!!\n";
                return;
            }

            pcl::copyPointCloud(*(pcloud_origin_),cloud_origin_);
            pcloud_ = pcloud_origin_;
        }

        FiltersHelper(CloudT &input_cloud):
        keyboard_flag_(-1),callKeyboardStop_(false){
            for (int i = 0; i < 6; ++i) {
                keyboard_value_[i] = free_value;
            }

            pcl::copyPointCloud(input_cloud,*pcloud_origin_);
            pcl::copyPointCloud(*(pcloud_origin_),cloud_origin_);
            pcloud_ = pcloud_origin_;
        }

        //Get cloud ptr
        CloudT_Ptr& GetOriginCloudPtr(){
            return pcloud_origin_;
        }
        CloudT_Ptr& GetCurrentCloudPtr(){
            return pcloud_;
        }

        void WriteToPCD(const CloudT_Ptr& pcloud, const string &output_path, bool binary = false){
            cout<<"Writing PointCloud to PCD-File.\n";
            pcl::PCDWriter writer;
            writer.write<pointT>(output_path,*pcloud_,binary);
            cout<<"PCD Output has already been written to destination.\n";
        }


        /**
         * Remove the outlier points by statistical method
         * @param src_cloud_ptr
         * @param dst_cloud_ptr
         * @param meanK  The number of neighbor-points that you want to check   REF: 20/50
         * @param StdDevMulThresh The threshold StdDev to judge one point as outlier  REF: 1.0
         * @param use_viewer  use visualization-tool or not
         * @return
         */
        bool RemoveOutlier_Statistical(CloudT_Ptr &src_cloud_ptr, CloudT_Ptr &dst_cloud_ptr,
                const int meanK, const double StdDevMulThresh, bool use_viewer = false){
            if(dst_cloud_ptr == nullptr){
                dst_cloud_ptr = CloudT_Ptr (new CloudT);
            }

            cout<<"Before StatisticalOutlierRemoval, there are "<<src_cloud_ptr->height*src_cloud_ptr->width<<" points in the cloud"<<endl;
            pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBA> sor;
            sor.setInputCloud(src_cloud_ptr);
            sor.setMeanK(meanK);
            sor.setStddevMulThresh(StdDevMulThresh);
            sor.filter(*dst_cloud_ptr);
            pcloud_ = dst_cloud_ptr;
            cout<<"After StatisticalOutlierRemoval, there are "<<pcloud_->height*pcloud_->width<<" points in the cloud"<<endl;

            if (use_viewer){
                VisualizeCloud(dst_cloud_ptr);
            }

            return true;
        }

        bool RemoveOutlier_Statistical(const int meanK, const double StdDevMulThresh,bool use_viewer = false){
            if(pcloud_ == nullptr){
                cout<<"You need to assure the FiltersHelper's current cloud is not empty.\n";
                return false;
            }
            CloudT_Ptr tmp(new CloudT());

            cout<<"Before StatisticalOutlierRemoval, there are "<<pcloud_->height*pcloud_->width<<" points in the cloud"<<endl;
            pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBA> sor;
            sor.setInputCloud(pcloud_);
            sor.setMeanK(meanK);
            sor.setStddevMulThresh(StdDevMulThresh);
            sor.filter(*tmp);
            pcloud_->swap(*tmp);

            cout<<"After StatisticalOutlierRemoval, there are "<<pcloud_->height*pcloud_->width<<" points in the cloud"<<endl;
            if (use_viewer){
                VisualizeCloud(pcloud_);
            }

            tmp.reset();
            if(pcloud_origin_ == pcloud_){
                pcloud_origin_ = boost::make_shared<CloudT>(cloud_origin_);
            }

            return true;
        }

        /**
         *  Limit Params set a range from selected axis, cut the points in or out of the area
         * @param axis
         * @param limit_min
         * @param limit_max
         * @param LimitsNegative  if true ,points in [limit_min,limit_max] will be deserted rather than reserved
         * @param use_viewer
         * @param pcloud
         */
        void PassThrough(CloudT_Ptr &pcloud, const std::string& axis,const double limit_min, const double limit_max,
                         bool LimitsNegative = false, bool use_viewer = false){
            if(pcloud == nullptr){
                pcloud = pcloud_;
            }

            bool axis_valid = (axis == "x" || axis == "y" || axis == "z");
            if(!axis_valid) {
                cout<<"PassThrough: Please input correct axis name,'x','y' or 'z'.\n";
                return;
            }

            CloudT_Ptr tmp(new CloudT());
            pcl::PassThrough<pointT> pass;
            pass.setInputCloud(pcloud);
            pass.setFilterFieldName(axis);
            pass.setFilterLimits(limit_min,limit_max);
            if(LimitsNegative)
                pass.setFilterLimitsNegative(true);
            pass.filter(*tmp);
            pcloud.swap(tmp);

            cout<<"After PassThrough, there are "<<pcloud->height*pcloud->width<<" points in the cloud"<<endl;

            if (use_viewer){
                VisualizeCloud(pcloud);
            }
            tmp.reset();

        }

        /**
         * Remove points in the area that selected by 3-Axis coordinates(in the range XYZ-[floor,ceil])
         * @param pcloud
         * @param x_floor
         * @param x_ceil
         * @param y_floor
         * @param y_ceil
         * @param z_floor
         * @param z_ceil
         * @param use_viewer
         */
        void ConditionalRemoval_3Axis(CloudT_Ptr &pcloud, const double x_floor, const double x_ceil,const double y_floor,
                                      const double y_ceil, const double z_floor, const double z_ceil,
                                      bool use_viewer = false){
            if(pcloud == nullptr){
                pcloud = pcloud_;
            }

            typedef pcl::ConditionAnd<pointT> ConditionAnd_;
            typedef boost::shared_ptr<pcl::ConditionAnd<pointT>> ConditionAnd_Ptr;
            CloudT_Ptr tmp (new CloudT());
            ConditionAnd_Ptr range_cond(new ConditionAnd_ ());

            range_cond->addComparison(pcl::FieldComparison<pointT>::ConstPtr(
                    new pcl::FieldComparison<pointT>("x", pcl::ComparisonOps::GT, x_floor)));
            range_cond->addComparison(pcl::FieldComparison<pointT>::ConstPtr(
                    new pcl::FieldComparison<pointT>("x", pcl::ComparisonOps::LT, x_ceil)));
            range_cond->addComparison(pcl::FieldComparison<pointT>::ConstPtr(
                    new pcl::FieldComparison<pointT>("y", pcl::ComparisonOps::GT, y_floor)));
            range_cond->addComparison(pcl::FieldComparison<pointT>::ConstPtr(
                    new pcl::FieldComparison<pointT>("y", pcl::ComparisonOps::LT, y_ceil)));
            range_cond->addComparison(pcl::FieldComparison<pointT>::ConstPtr(
                    new pcl::FieldComparison<pointT>("z", pcl::ComparisonOps::GT, z_floor)));
            range_cond->addComparison(pcl::FieldComparison<pointT>::ConstPtr(
                    new pcl::FieldComparison<pointT>("z", pcl::ComparisonOps::LT, z_ceil)));

            pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
            condrem.setCondition(range_cond);
            condrem.setInputCloud(pcloud);
            condrem.setKeepOrganized(true);
            condrem.filter(*tmp);
            pcloud.swap(tmp);

            if(use_viewer){
                VisualizeCloud(pcloud);
            }
            tmp.reset();

        }

        /**
         * Project the input cloud to a plane with preset coefficients
         * @param pcloud
         * @param plane_coeffs  Pointer of a float-type vector with 4 params that describe a plane
         * @param use_viewer
         */
        void ProjectToPlane(CloudT_Ptr& pcloud, const float *plane_coeffs, const bool use_viewer = false){

            if(pcloud == nullptr)
                pcloud = pcloud_;

            pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
            coefficients->values.resize (4);
            coefficients->values[0] = plane_coeffs[0];
            coefficients->values[1] = plane_coeffs[1];
            coefficients->values[2] = plane_coeffs[2];
            coefficients->values[3] = plane_coeffs[3];

            pcl::ProjectInliers<pcl::PointXYZRGBA> proj;
            proj.setModelType (pcl::SACMODEL_PLANE);
            proj.setInputCloud (pcloud);
            proj.setModelCoefficients (coefficients);
            proj.filter (*pcloud);

            if(use_viewer)
                VisualizeCloud(pcloud);

        }
        /**
         *  wait for KeyBoard input
         */
        void waitKeyboardFlag(){
            char tmp = -1;
            while(1) {
                tmp = cin.get();
                {
                    std::unique_lock<std::mutex> lck(mutex_kbf);
                    keyboard_flag_ = tmp;
                }
                if (tmp == 's')
                    break;
            }
        }

        void waitKeyboard(){
            double tmpV[6];
            char key_flag;
            string line;
            string sx0;
            while(1){

                if(callKeyboardStop_) {
                    callKeyboardStop_ = false;
                    break;
                }
                stringstream ss;
                getline(cin,line);
                ss << line;
                ss >> sx0;
                if(sx0 == "")
                    continue;

                //if there's only one input , it is supposed to receive a char flag
                if(ss.eof()){
                    key_flag = sx0[0];
                    sx0 = "";
                    {
                        std::unique_lock<std::mutex> lck(mutex_kbf);
                        keyboard_flag_ = key_flag;
                    }
                }else{
                    tmpV[0] = atof(sx0.c_str());
                    {
                        std::unique_lock<std::mutex> lck(mutex_kbv);
                        keyboard_value_[0] = tmpV[0];
                        for (int i = 1; i < 6; ++i) {
                            if(ss.eof())
                                keyboard_value_[i] = free_value;
                            else {
                                ss >> tmpV[i];
                                keyboard_value_[i] = tmpV[i];
                            }
                        }
                    }
                }
            }
            cout<<"Keyboard-Receiver ends.\n";
        }

        void VisualizeCloud(CloudT_Ptr &cloud_ptr){
            std::thread wait_stop(std::bind(&FiltersHelper::waitKeyboardFlag,this));
            char tmp = -1;

            cout<<"Visualizer: There are "<<cloud_ptr->width*cloud_ptr->height<<" points in the cloud.\n";
            cout<<"\n\tPress 's' to stop showing current cloud. \n";
            auto pviewer_ = new pcl::visualization::CloudViewer(default_viewer_name);

            while(tmp != 's') {
                {
                    std::unique_lock<std::mutex> lck(mutex_kbf);
                    tmp = keyboard_flag_;
                }
                pviewer_->showCloud(cloud_ptr);
                pviewer_->runOnVisualizationThreadOnce(viewerOneOffB);
            }
            {
                std::unique_lock<std::mutex> lck(mutex_kbf);
                keyboard_flag_ = -1;
                delete pviewer_;
            }
            wait_stop.join();
        }

        /**
         * Set a range on one axis , Distinguish the points by its position in the area or not
         * Inliers are shown as green , outliers are shown as red , Press 'c' or 'C' to cut the outliers
         * @param cloud  The Point-Cloud to be processed , if no arg, set it to the current cloud that filters-helper hold
         * @param axis  Name of your selected axis, could be 'x','y' or 'z'
         * @param floor  Min value of the range selected on the axis
         * @param ceil   Max value of the range selected on the axis
         * @return
         */
        void SelectRangeShowOnAxis(CloudT_Ptr &pcloud , const std::string& axis,
                                   const double floor = 0.0, const double ceil = 0.0){
            if(pcloud == nullptr)
                pcloud = pcloud_;

            pointT min3D;
            pointT max3D;
            pcl::getMinMax3D<pointT>(*pcloud,min3D,max3D);

            int count;
            double tmp_floor,tmp_ceil;
            char tmp_flag = -1;
            double last_tmp_floor = free_value, last_tmp_ceil = free_value;

            bool show_origin = false;
            if(fabs(floor)<1e-6 && fabs(ceil)<1e-6) {
                show_origin = true;
            }else{
                tmp_floor = floor;
                tmp_ceil = ceil;
                cout<<"The Original range on selected axis is: "<<tmp_floor<<" to "<<tmp_ceil<<endl;
            }

            std::thread wait_value(std::bind(&FiltersHelper::waitKeyboard,this));
            cout<<"\n\tInput two number to give the min and max value of selected range.\n"
            <<"\tGreen-Area: Inlier points\tRed-Area: Outlier points\n"
            <<"\tPress 's' to stop showing .\n"
            <<"\tPress 'c' to cut the cloud using the limits you set. \n"
            <<"\tPress 'C' to cut the cloud and stop showing. \n\n";
            auto pviewer = new pcl::visualization::CloudViewer(default_viewer_name);
            pviewer->showCloud(pcloud);
            pviewer->runOnVisualizationThreadOnce(viewerOneOffB);

            if(axis == "x"){
                cout<<"Axis X:\tMax value: "<<max3D.x<<"\tMin value: "<<min3D.x<<endl<<endl;
                if(show_origin){
                    tmp_floor = min3D.x;
                    tmp_ceil = max3D.x;
                }

                while( tmp_flag != 's' && tmp_flag != 'C') {
                    {
                        std::unique_lock<std::mutex> lck(mutex_kbf);
                        tmp_flag = keyboard_flag_;
                    }
                    if(tmp_flag == 'c'){
                        PassThrough(pcloud,axis,tmp_floor,tmp_ceil,false,false);
                        //If don't use a new viewer, the origin one will throw exception (possibly due to cloud's size change)
                        //I don't have other solutions to this problem
                        delete pviewer;
                        auto pviewer = new pcl::visualization::CloudViewer(default_viewer_name);
                        pviewer->runOnVisualizationThreadOnce(viewerOneOffB);
                        std::unique_lock<std::mutex> lck(mutex_kbf);
                        keyboard_flag_ = -1;
                    }
                    {
                        std::unique_lock<std::mutex> lck(mutex_kbv);
                        if((keyboard_value_[0] != last_tmp_floor || keyboard_value_[1] != last_tmp_ceil)&&
                           (keyboard_value_[0] != free_value && keyboard_value_[1] != free_value)) {
                            tmp_floor = keyboard_value_[0];
                            tmp_ceil = keyboard_value_[1];
                        }
                    }

                    pviewer->showCloud(pcloud);
                    if( tmp_floor == last_tmp_floor && tmp_ceil == last_tmp_ceil)
                        continue;
                    else {
                        count = 0;
                        for (size_t i = 0, iend = pcloud->size(); i < iend; ++i) {
                            pcl::PointXYZRGBA point_i = pcloud->at(i);
                            if (point_i.x < tmp_floor || point_i.x > tmp_ceil) {
                                pcloud->at(i).r = 255;
                                pcloud->at(i).g = 0;
                                pcloud->at(i).b = 0;
                            } else {
                                pcloud->at(i).r = 0;
                                pcloud->at(i).g = 255;
                                pcloud->at(i).b = 0;
                                count++;
                            }
                        }
                        last_tmp_ceil = tmp_ceil;
                        last_tmp_floor = tmp_floor;
                        cout << "In this Axis-X range, the number of points is: " << count << endl<<endl;
                    }
                }
            }
            if(axis == "y"){
                cout<<"Axis Y:\tMax value: "<<max3D.y<<"\tMin value: "<<min3D.y<<endl<<endl;
                if(show_origin){
                    tmp_floor = min3D.y;
                    tmp_ceil = max3D.y;
                }

                while(tmp_flag != 's' && tmp_flag != 'C') {
                    {
                        std::unique_lock<std::mutex> lck(mutex_kbf);
                        tmp_flag = keyboard_flag_;
                    }
                    if(tmp_flag == 'c'){
                        PassThrough(pcloud,axis,tmp_floor,tmp_ceil,false,false);
                        delete pviewer;
                        auto pviewer = new pcl::visualization::CloudViewer(default_viewer_name);
                        pviewer->runOnVisualizationThreadOnce(viewerOneOffB);
                        std::unique_lock<std::mutex> lck(mutex_kbf);
                        keyboard_flag_ = -1;
                    }
                    {
                        std::unique_lock<std::mutex> lck(mutex_kbv);
                        if((keyboard_value_[0] != last_tmp_floor || keyboard_value_[1] != last_tmp_ceil)&&
                           (keyboard_value_[0] != free_value && keyboard_value_[1] != free_value)) {
                            tmp_floor = keyboard_value_[0];
                            tmp_ceil = keyboard_value_[1];
                        }
                    }

                    pviewer->showCloud(pcloud);
                    if( tmp_floor == last_tmp_floor && tmp_ceil == last_tmp_ceil)
                        continue;
                    else {
                        count = 0;
                        for (size_t i = 0, iend = pcloud->size(); i < iend; ++i) {
                            pcl::PointXYZRGBA point_i = pcloud->at(i);
                            if (point_i.y < tmp_floor || point_i.y > tmp_ceil) {
                                pcloud->at(i).r = 255;
                                pcloud->at(i).g = 0;
                                pcloud->at(i).b = 0;
                            } else {
                                pcloud->at(i).r = 0;
                                pcloud->at(i).g = 255;
                                pcloud->at(i).b = 0;
                                count++;
                            }
                        }

                        last_tmp_ceil = tmp_ceil;
                        last_tmp_floor = tmp_floor;
                        cout << "In this Axis-Y range, the number of points is: " << count << endl<<endl;

                    }

                }
            }
            if(axis == "z"){
                cout<<"Axis Z:\tMax value: "<<max3D.z<<"\tMin value: "<<min3D.z<<endl<<endl;
                if(show_origin){
                    tmp_floor = min3D.z;
                    tmp_ceil = max3D.z;
                }

                while(tmp_flag != 's' && tmp_flag != 'C') {
                    {
                        std::unique_lock<std::mutex> lck(mutex_kbf);
                        tmp_flag = keyboard_flag_;
                    }
                    if(tmp_flag == 'c'){
                        PassThrough(pcloud,axis,tmp_floor,tmp_ceil,false,false);
                        delete pviewer;
                        auto pviewer = new pcl::visualization::CloudViewer(default_viewer_name);
                        pviewer->runOnVisualizationThreadOnce(viewerOneOffB);
                        std::unique_lock<std::mutex> lck(mutex_kbf);
                        keyboard_flag_ = -1;
                    }
                    {
                        std::unique_lock<std::mutex> lck(mutex_kbv);
                        if((keyboard_value_[0] != last_tmp_floor || keyboard_value_[1] != last_tmp_ceil)&&
                           (keyboard_value_[0] != free_value && keyboard_value_[1] != free_value)) {
                            tmp_floor = keyboard_value_[0];
                            tmp_ceil = keyboard_value_[1];
                        }
                    }

                    pviewer->showCloud(pcloud);
                    if( tmp_floor == last_tmp_floor && tmp_ceil == last_tmp_ceil)
                        continue;
                    else {
                        count = 0;
                        for (size_t i = 0, iend = pcloud->size(); i < iend; ++i) {
                            pcl::PointXYZRGBA point_i = pcloud->at(i);
                            if (point_i.z < tmp_floor || point_i.z > tmp_ceil) {
                                pcloud->at(i).r = 255;
                                pcloud->at(i).g = 0;
                                pcloud->at(i).b = 0;
                            } else {
                                pcloud->at(i).r = 0;
                                pcloud->at(i).g = 255;
                                pcloud->at(i).b = 0;
                                count++;
                            }
                        }
                        last_tmp_ceil = tmp_ceil;
                        last_tmp_floor = tmp_floor;
                        cout << "In this Axis-Z range, the number of points is: " << count << endl<<endl;
                    }
                }
            }

            if(tmp_flag == 's' || tmp_flag == 'C')
            {
                std::unique_lock<std::mutex> lck(mutex_call);
                callKeyboardStop_ = true;
            }

            wait_value.join();
            delete pviewer;
            {
                std::unique_lock<std::mutex> lck(mutex_kbv);
                for (int i = 0; i < 6; ++i) {
                    keyboard_value_[i] = free_value;
                }
                std::unique_lock<std::mutex> lck1(mutex_kbf);
                keyboard_flag_ = -1;
            }

            if (tmp_flag == 'C')
                PassThrough(pcloud,axis,tmp_floor,tmp_ceil,false,true);

        }

        /**
         * Set an area by 3-Axis Coordinate range [min,max],
         * Points in the area are shown as red, you can press 'c' to cut the red points
         * @param pcloud
         */
        void SelectedAreaCut(CloudT_Ptr& pcloud){

            if(pcloud == nullptr)
                pcloud = pcloud_;

            pointT min3D;
            pointT max3D;
            pcl::getMinMax3D<pointT>(*pcloud,min3D,max3D);

            int count;
            char tmp_flag;
            bool CUT = false;
            pcl::ExtractIndices<pointT> extract_;
            pcl::PointIndices::Ptr outliers(new pcl::PointIndices());
            outliers->indices.reserve(pcloud->size());
            //ORDER: x_min, x_max, y_min, y_max, z_min, z_max
            double tmpV[6];
            double last_tmpV[6];
            for (int i = 0; i < 6; ++i) {
                last_tmpV[i] = free_value;
            }

            std::thread wait_KeyBoard(std::bind(&FiltersHelper::waitKeyboard,this));
            cout<<"\n\tInput 6 number to select area: x_min,x_max,y_min,y_max,z_min,z_max.\n"
                <<"\tGreen-Area: Points not in this area \tRed-Area: Points in the selected area.\n"
                <<"\tPress 's' to stop showing .\n"
                <<"\tPress 'c' to cut the cloud using the limits you set. \n\n";
            auto pviewer = new pcl::visualization::CloudViewer(default_viewer_name);
            pviewer->runOnVisualizationThreadOnce(viewerOneOffB);

            tmpV[0] = min3D.x;    tmpV[1] = max3D.x;
            tmpV[2] = min3D.y;    tmpV[3] = max3D.y;
            tmpV[4] = min3D.z;    tmpV[5] = max3D.z;
            cout<<"The original cloud's 3-Axis limits are:\n"
                <<"X_min: "<<tmpV[0]<<"\tX_max: "<<tmpV[1]<<endl
                <<"Y_min: "<<tmpV[2]<<"\tY_max: "<<tmpV[3]<<endl
                <<"Z_min: "<<tmpV[4]<<"\tZ_max: "<<tmpV[5]<<endl<<endl;

            while( tmp_flag != 's') {
                {
                    std::unique_lock<std::mutex> lck(mutex_kbf);
                    tmp_flag = keyboard_flag_;
                }
                if(tmp_flag == 'c'){
                    CUT = true;
                    std::unique_lock<std::mutex> lck(mutex_kbf);
                    keyboard_flag_ = -1;
                }
                {
                    std::unique_lock<std::mutex> lck(mutex_kbv);
                    for (int i = 0; i < 6; ++i) {
                        if(keyboard_value_[i] != free_value)
                            tmpV[i] = keyboard_value_[i];
                    }
                }

                pviewer->showCloud(pcloud);

                //Check if there's anything new
                bool judge_new = true;
                for (int i = 0; i < 6; ++i) {
                    bool tmpB;
                    tmpB = (tmpV[i]==last_tmpV[i]);
                    judge_new = (judge_new && tmpB);
                }
                if(judge_new && !CUT)
                    continue;
                else {
                    count = 0;
                    for (size_t i = 0, iend = pcloud->size(); i < iend; ++i) {
                        pcl::PointXYZRGBA point_i = pcloud->at(i);
                        bool judge_area = (point_i.x < tmpV[0] || point_i.x > tmpV[1]
                                        || point_i.y < tmpV[2] || point_i.y > tmpV[3]
                                        || point_i.z < tmpV[4] || point_i.z > tmpV[5]);
                        if (judge_area) {
                            pcloud->at(i).r = 0;
                            pcloud->at(i).g = 255;
                            pcloud->at(i).b = 0;
                        } else {
                            count++;
                            if (CUT) {
                                outliers->indices.push_back(i);
                            }else{
                                pcloud->at(i).r = 255;
                                pcloud->at(i).g = 0;
                                pcloud->at(i).b = 0;
                            }
                        }
                    }

                    for (int i = 0; i < 6; ++i) {
                        last_tmpV[i] = tmpV[i];
                    }
                    if(CUT){
                        cout<<"before cut , cloud's size is "<<pcloud->size()<<endl;
                        extract_.setInputCloud(pcloud);
                        extract_.setIndices(outliers);
                        extract_.setNegative(true);
                        extract_.filter(*pcloud);
                        CUT = false;
                        cout<<"after cut , cloud's size is "<<pcloud->size()<<endl;
                        outliers->indices.clear();
                        cout << "All the points in the selected area have been cut, total: "<<count<<endl;
                        //If don't use a new viewer, the origin one will throw exception (possibly due to cloud's size change)
                        //I don't have other solutions to this problem
//                        delete pviewer;
//                        auto pviewer = new pcl::visualization::CloudViewer(default_viewer_name);
//                        pviewer->runOnVisualizationThreadOnce(viewerOneOffB);
                    }else
                        cout << "In this area(red), the number of points is: " << count << endl<<endl;
                }
            }

            {
                std::unique_lock<std::mutex> lck(mutex_call);
                callKeyboardStop_ = true;
            }
            wait_KeyBoard.join();
            delete pviewer;
            {
                std::unique_lock<std::mutex> lck(mutex_kbv);
                for (int i = 0; i < 6; ++i) {
                    keyboard_value_[i] = free_value;
                }
                std::unique_lock<std::mutex> lck1(mutex_kbf);
                keyboard_flag_ = -1;
            }
        }


        /**
         * Set an area by Y-Axis Coordinate range [min,max],
         * Points in the area are shown as red, you can press 'c' to cut the red points
         * @param pcloud
         */
        void SelectedYaxisCut(CloudT_Ptr& pcloud){

            if(pcloud == nullptr)
                pcloud = pcloud_;

            pointT min3D;
            pointT max3D;
            pcl::getMinMax3D<pointT>(*pcloud,min3D,max3D);

            int count;
            char tmp_flag;
            bool CUT = false;
            pcl::ExtractIndices<pointT> extract_;
            pcl::PointIndices::Ptr outliers(new pcl::PointIndices());
            outliers->indices.reserve(pcloud->size());
            //ORDER: x_min, x_max, y_min, y_max, z_min, z_max
            double tmpV[2];
            double last_tmpV[2];
            for (int i = 0; i < 2; ++i) {
                last_tmpV[i] = free_value;
            }

            std::thread wait_KeyBoard(std::bind(&FiltersHelper::waitKeyboard,this));
            cout<<"\n\tInput 2 number to select area: y_min,y_max.\n"
                <<"\tGreen-Area: Points not in this area \tRed-Area: Points in the selected area.\n"
                <<"\tPress 's' to stop showing .\n"
                <<"\tPress 'c' to cut the cloud using the limits you set. \n\n";
            auto pviewer = new pcl::visualization::CloudViewer(default_viewer_name);
            pviewer->runOnVisualizationThreadOnce(viewerOneOffB);

            tmpV[0] = min3D.y;    tmpV[1] = max3D.y;

            cout<<"The original cloud's 3-Axis limits are:\n"
                <<"Y_min: "<<tmpV[0]<<"\tY_max: "<<tmpV[1]<<endl<<endl;

            while( tmp_flag != 's') {
                {
                    std::unique_lock<std::mutex> lck(mutex_kbf);
                    tmp_flag = keyboard_flag_;
                }
                if(tmp_flag == 'c'){
                    CUT = true;
                    std::unique_lock<std::mutex> lck(mutex_kbf);
                    keyboard_flag_ = -1;
                }
                {
                    std::unique_lock<std::mutex> lck(mutex_kbv);
                    for (int i = 0; i < 2; ++i) {
                        if(keyboard_value_[i] != free_value)
                            tmpV[i] = keyboard_value_[i];
                    }
                }

                pviewer->showCloud(pcloud);

                //Check if there's anything new
                bool judge_new = true;
                for (int i = 0; i < 2; ++i) {
                    bool tmpB;
                    tmpB = (tmpV[i]==last_tmpV[i]);
                    judge_new = (judge_new && tmpB);
                }
                if(judge_new && !CUT)
                    continue;
                else {
                    count = 0;
                    for (size_t i = 0, iend = pcloud->size(); i < iend; ++i) {
                        pcl::PointXYZRGBA point_i = pcloud->at(i);
                        bool judge_area = (point_i.y < tmpV[0] || point_i.y > tmpV[1]);
                        if (judge_area) {
                            pcloud->at(i).r = 0;
                            pcloud->at(i).g = 255;
                            pcloud->at(i).b = 0;
                        } else {
                            count++;
                            if (CUT) {
                                outliers->indices.push_back(i);
                            }else{
                                pcloud->at(i).r = 255;
                                pcloud->at(i).g = 0;
                                pcloud->at(i).b = 0;
                            }
                        }
                    }

                    for (int i = 0; i < 2; ++i) {
                        last_tmpV[i] = tmpV[i];
                    }
                    if(CUT){
                        cout<<"before cut , cloud's size is "<<pcloud->size()<<endl;
                        extract_.setInputCloud(pcloud);
                        extract_.setIndices(outliers);
                        extract_.setNegative(true);
                        extract_.filter(*pcloud);
                        CUT = false;
                        cout<<"after cut , cloud's size is "<<pcloud->size()<<endl;
                        outliers->indices.clear();
                        cout << "All the points in the selected area have been cut, total: "<<count<<endl;
                        //If don't use a new viewer, the origin one will throw exception (possibly due to cloud's size change)
                        //I don't have other solutions to this problem
//                        delete pviewer;
//                        auto pviewer = new pcl::visualization::CloudViewer(default_viewer_name);
//                        pviewer->runOnVisualizationThreadOnce(viewerOneOffB);
                    }else
                        cout << "In this area(red), the number of points is: " << count << endl<<endl;
                }
            }

            {
                std::unique_lock<std::mutex> lck(mutex_call);
                callKeyboardStop_ = true;
            }
            wait_KeyBoard.join();
            delete pviewer;
            {
                std::unique_lock<std::mutex> lck(mutex_kbv);
                for (int i = 0; i < 2; ++i) {
                    keyboard_value_[i] = free_value;
                }
                std::unique_lock<std::mutex> lck1(mutex_kbf);
                keyboard_flag_ = -1;
            }
        }

        /**
         * Convert a point-cloud to a Occupied-Grid-Map
         * @param pcloud
         * @param grid_image
         * @param axisU  "x" please
         * @param axisV  "z" please
         * @param occupied_thresh
         * @param output_path
         * @param img_border
         * @param default_resolution
         * @param default_width
         * @param default_height
         */
        void CloudToGridImage(const CloudT_Ptr &pcloud, cv::Mat &grid_image, const string &axisU, const string &axisV ,
                              const int occupied_thresh, const string output_path = "",
                              const int img_border = 0, const double default_resolution = 0.0 ,
                              const int yes_value = 255, const int no_value = 0,
                              const int default_width = 1000, const int default_height = 1000){
            double resolution,resolution_inv;
            double cloud_width,cloud_height;
            int img_width, img_height;
            int min_width = 100, min_height = 100;
            if(fabs(default_resolution) < 1e-6)
                resolution = 0.02;
            else
                resolution = default_resolution;

            img_width = default_width;
            img_height = default_height;
            pointT min3D,max3D;
            pcl::getMinMax3D(*pcloud,min3D,max3D);

            if(axisU != "x" || axisV != "z"){
                cout<<"Sorry, don't know how to write simple code for calling different functions with different input strings.\n"
                    <<"And I haven't written the code for other cases, so please let the axisU,axisV be x,z.\n";
                return;
            }

            double axisU_max = max3D.x, axisU_min = min3D.x;
            double axisV_max = max3D.z, axisV_min = min3D.z;
            cloud_width = axisU_max - axisU_min;
            cloud_height = axisV_max - axisV_min;

            while((cloud_width / resolution > default_width) || (cloud_height / resolution > default_height)){
                resolution = 2.0 * resolution;
                cout<<"Adjusting resolution(*2.0)... : "<<resolution<<endl;
            }
            while((cloud_width / resolution < min_width) || (cloud_height / resolution < min_height)){
                resolution = 2.0 * resolution;
                cout<<"Adjusting resolution(*0.5)... : "<<resolution<<endl;
            }

            cout<<"\nFinal Resolution is: \t"<<resolution<<endl<<endl;
            resolution_inv = 1.0/resolution;
            img_width = ceil(cloud_width * resolution_inv);
            img_height = ceil(cloud_height * resolution_inv);
            cout<<img_width<<"\t"<<img_height<<endl;
            vector<vector<vector<int>>> Grid(img_width,vector<vector<int>>(img_height));

            for(size_t i = 0, iend = pcloud->size(); i < iend; ++i){
                pointT point = pcloud->at(i);
                int index_w = floor((double)(point.x - axisU_min)*resolution_inv);
                int index_h = floor((double)(point.z - axisV_min)*resolution_inv);
                Grid[index_w][index_h].push_back(i);
            }

            double trans_w = (double)(0.0 - axisU_min) + img_border*resolution;
            double trans_h = (double)(0.0 - axisV_min) + img_border*resolution;
            cout<<"Translation between origin-cloud-frame and grid-map-frame is: \n"
            <<"x: "<<trans_w<<"\ty: "<<trans_h<<endl;
            trans_w = (double)(axisU_max) + img_border*resolution;
            trans_h = (double)(axisV_max) + img_border*resolution;
            cout<<"\n Or (if you rotate the map n*90 deg)---\nx: "<<trans_w<<"\ty: "<<trans_h<<endl;

            //make image an occupied-grid-map
            grid_image = cv::Mat(img_height+2*img_border,img_width+2*img_border,CV_8UC1);
            for (int i = 0, iend = img_width+2*img_border; i < iend; ++i) {
                for (int j = 0, jend = img_height+2*img_border; j < jend; ++j) {
                    if(i < img_border || j < img_border
                    || i > img_width + img_border -1 || j > img_height + img_border -1){
                        grid_image.at<uchar>(j,i) = no_value;
                        continue;
                    }

                    if (Grid[i-img_border][j-img_border].size() < occupied_thresh)
                        grid_image.at<uchar>(j,i) = no_value;
                    else
                        grid_image.at<uchar>(j,i) = yes_value;
                }
            }

            //翻转图片
            cv::Mat mirror = grid_image.clone();
            const int num_cols = mirror.cols;
            grid_image = cv::Mat(grid_image.rows, grid_image.cols, grid_image.type());
            for(int i=0; i<grid_image.rows; ++i){
                for(int j=0; j<grid_image.cols; ++j){
                    grid_image.at<uchar>(i,j) = mirror.at<uchar>(i, num_cols-1-j);
                }
            }

            cv::imshow("Grid-Image",grid_image);
            cv::waitKey(0);
            if(output_path != ""){
                // save as 无压缩png
                std::vector<int> compression_params;
                compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
                compression_params.push_back(0);
                compression_params.push_back(cv::IMWRITE_PNG_STRATEGY);
                compression_params.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);

                cv::imwrite(output_path+"map.png", grid_image, compression_params);
            }

        }

        /**
         * 分割出一个平面，并计算其参数
         * @param pcloud
         * @param disTh
         * @param use_viewer
         * @return
         */
        Eigen::Vector4d SinglePlaneSegmentation(CloudT_Ptr &pcloud, const double disTh, const bool use_viewer){

            pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
//            pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
            pcl::PointIndices inliers;
            pcl::SACSegmentation<pointT> seg;
            seg.setOptimizeCoefficients(true);
            seg.setModelType(pcl::SACMODEL_PLANE);
            seg.setMethodType(pcl::SAC_RANSAC);
            seg.setDistanceThreshold(disTh);
            seg.setInputCloud(pcloud);
            seg.segment(inliers, *coefficients);

            pcl::ExtractIndices<pointT> extract;
            extract.setInputCloud(pcloud);
            extract.setNegative(false);

            CloudT_Ptr tmp(new pcl::PointCloud<pointT>);
            extract.setIndices(boost::make_shared<pcl::PointIndices>(inliers));
            extract.filter(*tmp);
            pcloud->swap(*tmp);

            if(coefficients->values[1] < 0)
                for(int i=0; i<4; ++i){
                    coefficients->values[i] = - coefficients->values[i];
                }

            Eigen::Vector4d coefs(coefficients->values[0],coefficients->values[1],coefficients->values[2],coefficients->values[3]);
            cout<<"The plane-segmentation 's coeffcients are:\n"
            <<coefs(0)<<"\t"<<coefs(1)<<"\t"<<coefs(2)<<"\t"<<coefs(3)<<endl;

            if(use_viewer)
                VisualizeCloud(pcloud);

            return coefs;

        }


    private:
        CloudT_Ptr pcloud_origin_;
        CloudT_Ptr pcloud_;
        CloudT cloud_origin_;

        //Keyboard-control variables
        char keyboard_flag_;
        bool callKeyboardStop_;
        double keyboard_value_[6];

        std::mutex mutex_kbf;
        std::mutex mutex_kbv;
        std::mutex mutex_call;
    };


    void FilterByHSV(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr color_cloud, const int h_min, const int h_max,
                     const double s_min = -1.0, const double s_max = -1.0, const double v_min = -1.0, const double v_max = -1.0){

        auto points = color_cloud->points;

        pcl::PointIndices inliers;
        pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
        extract.setInputCloud(color_cloud);
        extract.setNegative(false);
        for(int i=0; i<points.size(); ++i){

            Eigen::Vector3d hsv = RGBtoHSV(points[i].r, points[i].g, points[i].b);
            if(h_max > 0){
                if(hsv[0] < h_min || hsv[0] > h_max)
                    continue;
            }
            if(s_max > 0){
                if(hsv[1] < s_min || hsv[1] > s_max)
                    continue;
            }
            if(v_max > 0){
                if(hsv[2] < v_min || hsv[2] > v_max)
                    continue;
            }
            inliers.indices.push_back(i);
        }

//        cout<<inliers.indices.size()<<endl;
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGBA>);
        extract.setIndices(boost::make_shared<pcl::PointIndices>(inliers));
        extract.filter(*tmp);
        color_cloud->swap(*tmp);

    }


//namespace PointcloudProcessing
}

//namespace CV_Assistant
}
#endif //CV_ASSISTANT_FILTERUTILITIES_H
